#include "bewegen.h"
/* Hier sind alle Funktionsprototypen deklariert */

// Globale Variablen: xa, ya, k

/*
-------------------------------------------------------
Funktion "void bewegen.." führt die Bewegung des Roboters,
die der Werkzeuge sowie die der Bleche aus
-------------------------------------------------------
xe: x-Koordinate der Endstellung des Roboterarms
ye: y-Koordinate der Endstellung des Roboterarms
g: Greifervariable	*0: kein Werkzeug im Greifer
					*1: offene Hand
					*2: geschlossene Hand
					*3: Schweißgerät
xse: x-Koordinate der anzufahrenden Schienenposition
t_max: Zeitdauer der Roboterbewegung in [ms]
f1,f2: Fließbandvariablen	*0: Blech im Greifer
							*1: Blech auf Eingangsfließband
							*2: Blech auf Ausgangsfließband
					
*/

void bewegen(HWND hwnd, HDC hdc, RECT rect, int xe, int ye, int g, int xse, int t_max, int f1, int f2)
{
	int anz_loes;
	int i, t;
	int t_step = 40;			//dt in ms
	double xsa;				//x-Ausgangsposition des Roboters auf der Schiene
	double sollpunkt[2];
	double xa, ya;
	double winkel1[2], winkel2[2];

	xa = robo2_n[9][0];
	ya = robo2_n[9][1];

	xsa = robo1_n[4][0];

/*Roboterarmtransfomation:	*/

	for (t=0; t<(t_max); t=t+(t_step))
	{
	
		sollpunkt[0]=xa + t*(xe-xa)/(t_max) - robo1[4][0] - robo1_n[4][0] + (t_step * (xse-xsa) / t_max);  
		sollpunkt[1]=ya + t*(ye-ya)/(t_max) - robo1[4][1] - robo1_n[4][1]; 

		anz_loes=SuchenWinkel(sollpunkt, l, l, winkel1, winkel2);

	
		for (i=0;i<6;i++)		//Transformation 1. Arm
			TrafoPoint(robo1[i], robo1_n[i], winkel1[0], robo1[4], robo1_n[4]);


		for (i=0;i<10;i++)		//Transformation 2. Arm
			TrafoPoint(robo2[i], robo2_n[i],  winkel2[0],  robo2[8],  robo1_n[5]);
		
		
		switch(g)			//Werkzeugtransformation
		{
		
		case 1:
			h=1;
			for (i=0;i<19;i++)
				TrafoPoint(hand[i], hand_n[i], winkel2[0], hand[18], robo2_n[9]);
			break;
	
		case 2:
			h=0;
			for (i=0;i<19;i++)
				TrafoPoint(hand[i], hand_n[i], winkel2[0], hand[18], robo2_n[9]);
			break;

		case 3:
			for (i=0;i<9;i++)
				TrafoPoint(schweisser[i], schweisser_n[i], winkel2[0], schweisser_n[0], robo2_n[9]);
			break;

		} //switch(g)..

	
		if(f1==0)			//1. Blech im Greifer
		{
			for (i=0;i<anz_p_b1;i++)
			{
				blech1_n[i][0] = blech1[i][0] + robo2_n[9][0];
				blech1_n[i][1] = blech1[i][1] + robo2_n[9][1];
			}
		}
		else if (f1==1)		//1. Blech auf Eingangsfließband	
			for (i=0;i<anz_p_b1;i++)
				blech1_n[i][0] = t_step * 350 / (t_max);

	
		else if (f1==2)		//1. Blech auf Ausgangsfließband
			for (i=0;i<anz_p_b1;i++)			
				blech1_n[i][0] = t_step * 450 / (t_max);
						
			
		if(f2==0)			//2. Blech im Greifer
		{
			for (i=0;i<anz_p_b2;i++)
			{
				blech2_n[i][0] = blech2[i][0] + robo2_n[9][0];
				blech2_n[i][1] = blech2[i][1] + robo2_n[9][1];
			}
		}
		else if (f2==1)		//1. Blech auf Eingangsfließband
		{
			for (i=0;i<anz_p_b2;i++)
				blech1_n[i][0] = t_step * 350 / (t_max);
		}
		else if (f2==2)		//2. Blech auf Ausgangsfließband
		{
			for (i=0;i<anz_p_b2;i++)
				blech2_n[i][0] = t_step * 450 / (t_max);
		}
	
		
/*Roboterarmtranslation auf Schiene:	*/
			
		if (xse != xsa)
		{
		

			for(i=0; i<6; i++)				//Translation Arm1
				robo1_n[i][0] += (t_step * (xse-xsa) / t_max);
			
			for(i=0; i<10; i++)				//Translation Arm2
				robo2_n[i][0] += (t_step * (xse-xsa) / t_max);
			

			switch(g)		//Werkzeugtranslation
			{

			case 1:
				h=1;								//Hand offen
				for (i=0;i<19;i++)
					hand_n[i][0] += (t_step * (xse-xsa) / t_max);
				break;

			case 2:
				h=0;								//Hand geschlossen
				for (i=0;i<19;i++)
					hand_n[i][0] += (t_step * (xse-xsa) / t_max);
				break;

			case 3:									//Schweißer inaktiv
				for (i=0;i<9;i++)
					schweisser_n[i][0] += (t_step * (xse-xsa) / t_max);
				break;
			
			
			} //switch(g)..
		
			if(f1==0)			//1. Blech im Greifer
				for (i=0;i<anz_p_b1;i++)
					blech1_n[i][0] += (t_step * (xse-xsa) / t_max);
		
		
			if(f2==0)			//2. Blech im Greifer
				for (i=0;i<anz_p_b2;i++)
					blech2_n[i][0] += (t_step * (xse-xsa) / t_max);
		
		} //if (xse != xsa)...


		InvalidateRect(hwnd, NULL, TRUE);
		hdc = BeginPaint (hwnd, &ps);
		 
		Zeichne_Statische_Elemente(hdc);
		Zeichne_Dynamische_Elemente(hdc);
		
		Sleep(t_step);


	} //for t..

} //void bewegen..
